{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Hand Following example #"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this notebook, you will use Pypot and an Inverse Kinematics toolbox to make Torso's hands follow each other."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Your Torso has two arms, and you can use simple methods to get and set the position of each hand."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Requirements"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You will need a fully functioning torso, either IRL or in simulator (V-REP).  \n",
    "More info [here](https://github.com/poppy-project/poppy-torso)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# The experiment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To be more precise, we will tell the right hand to keep a constant distance with the moving left hand, like on the picture above : "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![Example](assets/hand_follow.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The left arm will be compliant, so you can move it and watch the right arm following it."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Setting up the robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We begin by configuring the robot, to fit our needs for the experiment."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Begin with some useful imports : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "import time\n",
    "import numpy as np\n",
    "from pypot.creatures import PoppyTorso"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then, create your Pypot robot : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "poppy = PoppyTorso()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Initialize your robot positions to 0 : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "for m in poppy.motors:\n",
    "    m.goto_position(0, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The left arm must be compliant (so you can move it), and the right arm must be active"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# Left arm is compliant, right arm is active\n",
    "for m in poppy.l_arm:\n",
    "    m.compliant = False\n",
    "\n",
    "for m in poppy.r_arm:\n",
    "    m.compliant = False\n",
    "\n",
    "# The torso itself must not be compliant\n",
    "for m in poppy.torso:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Following the left hand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To follow the left hand, the script will do the following steps : \n",
    "* Find the 3D position of the left hand, with Forward Kinematics\n",
    "* Assign this position ( + a gap to avoid collision) as the target of the right hand\n",
    "* Tell the right hand to reach this target"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That's exactly what we do in the `hand_follow` function : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "def follow_hand(poppy, delta):\n",
    "    \"\"\"Tell the right hand to follow the left hand\"\"\"\n",
    "    right_arm_position = poppy.l_arm_chain.end_effector + delta\n",
    "    poppy.r_arm_chain.goto(right_arm_position, 0.5, wait=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, do this repeatedly in a loop : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "try:\n",
    "    while True:\n",
    "        follow_hand(poppy, target_delta)\n",
    "        time.sleep(delay_time)\n",
    "\n",
    "# Close properly the object when finished\n",
    "except KeyboardInterrupt:\n",
    "    poppy.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And you're done! Now you can move the left arm of the robot and see the right arm following."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# About the libraries\n",
    "* The Inverse Kinematics functionalities are provided by the [IKPy](https://github.com/Phylliade/ikpy) library\n",
    "\n",
    "* The [robot](https://www.poppy-project.org/creatures/poppy-torso/) used here is part of the [poppy-project](https://www.poppy-project.org), which also provides the motor-control library, [Pypot](https://poppy-project.github.io/pypot)."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
